/**
 * @file p_queue.c
 * @author Zhaoquan.xiong (zhaoquan@163.com)
 * @brief Implements a priority queue system with multiple priority levels, supporting thread-safe operations.
 *        It provides memory pool management and priority-based queue operations such as enqueue, dequeue, and queries.
 * @version 0.1
 * @date 2025-01-21
 * 
 * @copyright Copyright (c) 2025
 * 
 */

#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include "p_queue.h"



// Log macros
#define TAG "p_queue: "
#define p_queue_debug_log(TAG, ...) do{printf("%s", TAG); printf(__VA_ARGS__); printf("\n");} while(0)
#define p_queue_info_log(TAG, ...)  do{printf("%s", TAG); printf(__VA_ARGS__); printf("\n");} while(0)

// Memory allocation and freeing
/**
 * @brief Allocates memory of a given size.
 *
 * @param size The size of memory to allocate.
 * @return A pointer to the allocated memory, or NULL if allocation fails.
 */
static void *p_queue_malloc(size_t size) {
    if (size == 0) return NULL;
    void *mem = malloc(size);
    if (!mem) {
        p_queue_debug_log(TAG, "Memory allocation failed for size: %zu", size);
    }
    return mem;
}

/**
 * @brief Frees allocated memory.
 *
 * @param mem A pointer to the memory to free.
 */
static void p_queue_free(void *mem) {
    if (mem) {
        free(mem);
    }
}
/**
 * @brief Initialize the mutex.
 *
 * Initializes the mutex to an unlocked state.
 * 
 * @param mutex The mutex object to initialize.
 */
static void p_queue_mutex_init(p_queue_mutex_t *mutex) {
    if (mutex) {
        mutex->locked = false;  // Mutex is initially unlocked
    }
}
/**
 * @brief Acquire the mutex lock.
 *
 * This function blocks until the mutex is successfully acquired.
 * It uses interrupt disable to simulate lock acquisition and prevent interrupt interference.
 * 
 * @param mutex The mutex object to acquire.
 */
static void p_queue_mutex_lock(p_queue_mutex_t *mutex) {
    if (mutex){
        // while (mutex->locked) {
        //     // Mutex is locked, so wait until it's unlocked.
        //     // __enable_irq();  ///< Enable interrupts temporarily
        //     // __disable_irq(); ///< Disable interrupts again to recheck the lock status
        // }

        // Acquire the lock
        mutex->locked = true;
    }
}

/**
 * @brief Release the mutex lock.
 *
 * This function unlocks the mutex and enables interrupts again.
 * 
 * @param mutex The mutex object to release.
 */
static void p_queue_mutex_unlock(p_queue_mutex_t *mutex) {
    if (mutex){
        // Release the lock
        mutex->locked = false;
    }
}
/**
 * @brief Creates a priority queue with the specified number of priorities and maximum size.
 *
 * @param num_priorities The number of priorities.
 * @param max_size The maximum size of each priority queue.
 * @param data_type_size The size of the data type.
 * @return A pointer to the created priority queue, or NULL if creation fails.
 */
p_queue_t* p_queue_create(int num_priorities, int max_size, int data_type_size) {
    p_queue_t *pq = (p_queue_t*) p_queue_malloc(sizeof(p_queue_t));  // Allocate memory for the queue object
    if (!pq) {
        p_queue_debug_log(TAG, "Memory allocation for p_queue_t failed.");
        return NULL;
    }

    pq->num_priorities = num_priorities;
    pq->max_size = max_size;
    pq->data_type_size = data_type_size;
    pq->total_size = 0;

    // Dynamically allocate memory for priority queues
    pq->priority_queues = (p_queue_node_t*) p_queue_malloc(num_priorities * sizeof(p_queue_node_t));
    if (pq->priority_queues == NULL) {
        p_queue_debug_log(TAG, "Memory allocation for priority queues failed.");
        p_queue_free(pq);
        return NULL;
    }

    // Dynamically allocate memory for each priority queue
    for (int i = 0; i < num_priorities; i++) {
        pq->priority_queues[i].queue = (p_queue_data_node_t*) p_queue_malloc(max_size * sizeof(p_queue_data_node_t));
        if (pq->priority_queues[i].queue == NULL) {
            p_queue_debug_log(TAG, "Memory allocation for priority queue %d failed.", i);
            for (int j = 0; j < i; j++) {
                p_queue_free(pq->priority_queues[j].queue);
            }
            p_queue_free(pq->priority_queues);
            p_queue_free(pq);
            return NULL;
        }
        pq->priority_queues[i].front = 0;
        pq->priority_queues[i].rear = 0;
        pq->priority_queues[i].size = 0;
    }

    // Dynamically allocate memory pool and flags
    pq->memory_pool_size = max_size * data_type_size;
    pq->memory_pool = p_queue_malloc(pq->memory_pool_size);
    if (pq->memory_pool == NULL) {
        p_queue_debug_log(TAG, "Memory allocation for memory pool failed.");
        for (int i = 0; i < num_priorities; i++) {
            p_queue_free(pq->priority_queues[i].queue);
        }
        p_queue_free(pq->priority_queues);
        p_queue_free(pq);
        return NULL;
    }

    pq->memory_pool_used = (bool *) p_queue_malloc(max_size * sizeof(bool));
    if (pq->memory_pool_used == NULL) {
        p_queue_debug_log(TAG, "Memory allocation for memory pool usage flags failed.");
        p_queue_free(pq->memory_pool);
        for (int i = 0; i < num_priorities; i++) {
            p_queue_free(pq->priority_queues[i].queue);
        }
        p_queue_free(pq->priority_queues);
        p_queue_free(pq);
        return NULL;
    }

    // Initialize memory pool usage flags
    for (int i = 0; i < max_size; i++) {
        pq->memory_pool_used[i] = false;
    }
    // Initialize the mutex
    p_queue_mutex_init(&pq->queue_mutex);
    p_queue_info_log(TAG, "Priority queue created with %d priorities.", num_priorities);
    return pq;
}

/**
 * @brief Destroys the priority queue, freeing all allocated memory.
 *
 * @param pq A pointer to the priority queue pointer to destroy.
 */
void p_queue_destroy(p_queue_t **pq) {
    if (!pq || !*pq)
    {
        return;
    }
    p_queue_t *f_pq = *pq;
    p_queue_info_log(TAG, "Destroying priority queue.");
    for (int i = 0; i < f_pq->num_priorities; i++) {
        p_queue_free(f_pq->priority_queues[i].queue);
    }
    p_queue_free(f_pq->priority_queues);
    p_queue_free(f_pq->memory_pool);
    p_queue_free(f_pq->memory_pool_used);
    p_queue_free(f_pq);
    *pq = NULL;
    p_queue_info_log(TAG, "Priority queue destroyed.");
}

/**
 * @brief Checks if the specified priority queue is empty.
 *
 * @param pq The priority queue object.
 * @param priority The priority level to check.
 * @return true if the queue is empty, false otherwise.
 */
bool p_queue_is_empty(p_queue_t *pq, int priority) {
    if (!pq) {
        p_queue_debug_log(TAG, "Invalid priority queue pointer.");
        return true;
    }
    if (priority < 0 || priority >= pq->num_priorities) {
        p_queue_debug_log(TAG, "Invalid priority level: %d", priority);
        return true;  // Invalid priority returns empty
    }
    return pq->priority_queues[priority].size == 0;
}

/**
 * @brief Checks if the specified priority queue is full.
 *
 * @param pq The priority queue object.
 * @param priority The priority level to check.
 * @return true if the queue is full, false otherwise.
 */
bool p_queue_is_full(p_queue_t *pq, int priority) {
    if (!pq) {
        p_queue_debug_log(TAG, "Invalid priority queue pointer.");
        return true;
    }
    if (priority < 0 || priority >= pq->num_priorities) {
        p_queue_debug_log(TAG, "Invalid priority level: %d", priority);
        return true;  // Invalid priority returns full
    }
    return pq->priority_queues[priority].size == pq->max_size;
}

/**
 * @brief Gets the size of the specified priority queue.
 *
 * @param pq The priority queue object.
 * @param priority The priority level to query.
 * @return The size of the queue, or -1 if the priority is invalid.
 */
int p_queue_get_size(p_queue_t *pq, int priority) {
    if (!pq) {
        p_queue_debug_log(TAG, "Invalid priority queue pointer.");
        return -1;
    }
    if (priority < 0 || priority >= pq->num_priorities) {
        return -1;  // Invalid priority returns -1
    }
    return pq->priority_queues[priority].size;
}

/**
 * @brief Gets the total size of all queues combined.
 *
 * @param pq The priority queue object.
 * @return The total size of all queues.
 */
int p_queue_get_total_size(p_queue_t *pq) {
    if (!pq) {
        p_queue_debug_log(TAG, "Invalid priority queue pointer.");
        return -1;
    }
    return pq->total_size;
}

/**
 * @brief Finds a free index in the memory pool for storing data.
 *
 * @param pq The priority queue object.
 * @return The index of the free memory slot, or -1 if no slots are available.
 */
int p_queue_find_free_memory_index(p_queue_t *pq) {
    for (int i = 0; i < pq->max_size; i++) {
        if (!pq->memory_pool_used[i]) {
            pq->memory_pool_used[i] = true;
            return i;
        }
    }
    return -1;  // No free memory slot
}

/**
 * @brief Enqueues data into the priority queue with the specified priority.
 *
 * @param pq The priority queue object.
 * @param data The data to enqueue.
 * @param priority The priority level of the queue to insert data into.
 * @return The status of the operation.
 */
p_queue_status_t p_queue_send(p_queue_t *pq, void *data, int priority) {
    if (!pq || !data) {
        p_queue_debug_log(TAG, "Invalid priority queue or data pointer.");
        return P_QUEUE_ERROR_INVALID;
    }
    // Acquire the mutex lock to ensure thread safety
    p_queue_mutex_lock(&pq->queue_mutex);
    if (p_queue_is_full(pq, priority)) {
        p_queue_mutex_unlock(&pq->queue_mutex);  // Release the lock
        p_queue_debug_log(TAG, "Queue %d is full.", priority);
        return P_QUEUE_ERROR_FULL;
    }
    
    int free_index = p_queue_find_free_memory_index(pq);
    if (free_index == -1) {
        p_queue_mutex_unlock(&pq->queue_mutex);  // Release the lock
        return P_QUEUE_ERROR_MEMORY;
    }

    // Copy data into memory pool
    void *memory_slot = (char *)pq->memory_pool + free_index * pq->data_type_size;
    memcpy(memory_slot, data, pq->data_type_size);

    // Add element to the queue
    p_queue_node_t *queue = &pq->priority_queues[priority];
    p_queue_data_node_t *node = &queue->queue[queue->rear];
    node->priority = priority;
    node->index = queue->rear;
    node->data_index = free_index;

    queue->rear = (queue->rear + 1) % pq->max_size;
    queue->size++;
    pq->total_size++;
    p_queue_mutex_unlock(&pq->queue_mutex);  // Release the lock
    return P_QUEUE_SUCCESS;
}

/**
 * @brief Sends data to the highest priority queue (priority 0).
 *
 * @param pq The priority queue object.
 * @param data A pointer to the data to be inserted.
 * @return A status code indicating success or failure.
 */
p_queue_status_t p_queue_send_highest_priority(p_queue_t *pq, void *data) {
    if (!pq || !data) {
        p_queue_debug_log(TAG, "Invalid priority queue or data pointer.");
        return P_QUEUE_ERROR_INVALID;
    }

    // Always send to the highest priority (priority 0)
    return p_queue_send(pq, data, 0);
}

/**
 * @brief Sends data to the lowest priority queue (the last priority).
 *
 * @param pq The priority queue object.
 * @param data A pointer to the data to be inserted.
 * @return A status code indicating success or failure.
 */
p_queue_status_t p_queue_send_lowest_priority(p_queue_t *pq, void *data) {
    if (!pq || !data) {
        p_queue_debug_log(TAG, "Invalid priority queue or data pointer.");
        return P_QUEUE_ERROR_INVALID;
    }

    // Always send to the lowest priority (priority num_priorities - 1)
    return p_queue_send(pq, data, pq->num_priorities - 1);
}

/**
 * @brief Dequeues data from the priority queue.
 *
 * @param pq The priority queue object.
 * @param data A pointer to the buffer where the dequeued data will be stored.
 * @return The status of the operation.
 */
p_queue_status_t p_queue_receive(p_queue_t *pq, void **data) {
    if (!pq || !data) {
        p_queue_debug_log(TAG, "Invalid priority queue or data pointer.");
        return P_QUEUE_ERROR_INVALID;
    }
    
    // Acquire the mutex lock to ensure thread safety
    p_queue_mutex_lock(&pq->queue_mutex);
    // Traverse all priorities, starting from the highest priority
    for (int priority = 0; priority < pq->num_priorities; priority++) {
        // Check if the current priority queue is not empty
        if (!p_queue_is_empty(pq, priority)) {
            p_queue_node_t *q = &pq->priority_queues[priority];
            p_queue_data_node_t node = q->queue[q->front];

            // Retrieve the data from the memory pool
            *data = (char *) pq->memory_pool + node.data_index * pq->data_type_size;

            // Mark the memory pool slot as unused
            pq->memory_pool_used[node.data_index] = false;

            // Move the front pointer and update the queue size
            q->front = (q->front + 1) % pq->max_size;
            q->size--;
            pq->total_size --;
            // Release the mutex lock
            p_queue_mutex_unlock(&pq->queue_mutex);
            p_queue_info_log(TAG, "Received data from priority %d, remaining size: %d.", priority, pq->total_size);
            return P_QUEUE_SUCCESS;
        }
    }

    p_queue_mutex_unlock(&pq->queue_mutex);  // Release the lock
    p_queue_debug_log(TAG, "All queues are empty.");
    return P_QUEUE_ERROR_EMPTY;
}

/**
 * @brief Peeks at the data from the highest priority queue that contains data, without removing it.
 *
 * @param pq The target priority queue object.
 * @param data A pointer to store the peeked data.
 * @return P_QUEUE_SUCCESS if successful, otherwise returns the corresponding error code.
 */
p_queue_status_t p_queue_peek(p_queue_t *pq, void **data) {
    if (!pq || !data) {
        p_queue_debug_log(TAG, "Invalid priority queue or data pointer.");
        return P_QUEUE_ERROR_INVALID;
    }

    // Acquire the mutex lock to ensure thread safety
    p_queue_mutex_lock(&pq->queue_mutex);
    // Traverse all priorities, starting from the highest priority
    for (int priority = 0; priority < pq->num_priorities; priority++) {
        // Check if the current priority queue is not empty
        if (!p_queue_is_empty(pq, priority)) {
            p_queue_node_t *q = &pq->priority_queues[priority];
            p_queue_data_node_t node = q->queue[q->front];

            // Retrieve the data from the memory pool (without removing it)
            *data = (char *) pq->memory_pool + node.data_index * pq->data_type_size;

            p_queue_info_log(TAG, "Peeked data from priority %d, data_index: %d.", priority, node.data_index);
            p_queue_mutex_unlock(&pq->queue_mutex);  // Release the lock
            return P_QUEUE_SUCCESS;
        }
    }
    p_queue_mutex_unlock(&pq->queue_mutex);  // Release the lock
    p_queue_debug_log(TAG, "All queues are empty.");
    return P_QUEUE_ERROR_EMPTY;
}

/**
 * @brief Clears the specified priority queue, freeing all elements in it.
 *
 * @param pq The priority queue object.
 * @param priority The priority level to clear.
 * @return A status code indicating success or failure.
 */
p_queue_status_t p_queue_clear(p_queue_t *pq, int priority) {
    if (!pq) {
        p_queue_debug_log(TAG, "Invalid priority queue pointer.");
        return P_QUEUE_ERROR_INVALID;
    }

    if (priority < 0 || priority >= pq->num_priorities) {
        p_queue_debug_log(TAG, "Invalid priority level: %d", priority);
        return P_QUEUE_ERROR_INVALID;
    }
    // Acquire the mutex lock to ensure thread safety
    p_queue_mutex_lock(&pq->queue_mutex);

    p_queue_node_t *q = &pq->priority_queues[priority];
    while (!p_queue_is_empty(pq, priority)) {
        p_queue_data_node_t node = q->queue[q->front];
        pq->memory_pool_used[node.data_index] = false;  // Mark memory pool slot as free
        q->front = (q->front + 1) % pq->max_size;
        q->size--;
    }
    pq->total_size = 0;
    // Release the mutex lock
    p_queue_mutex_unlock(&pq->queue_mutex);
    p_queue_info_log(TAG, "Cleared all elements from queue %d.", priority);
    return P_QUEUE_SUCCESS;
}
/**
 * @brief Dumps the current status of the priority queue (for debugging purposes).
 *
 * @param pq The priority queue object.
 */
void p_queue_dump(p_queue_t *pq) {
    if (!pq) {
        p_queue_debug_log(TAG, "Invalid priority queue pointer.");
        return;
    }
    // Acquire the mutex lock to ensure thread safety
    p_queue_mutex_lock(&pq->queue_mutex);
    p_queue_info_log(TAG, "Dumping status of priority queue.");
    for (int i = 0; i < pq->num_priorities; i++) {
        p_queue_node_t *q = &pq->priority_queues[i];
        p_queue_info_log(TAG, "Priority queue %d: size = %d, front = %d, rear = %d", i, q->size, q->front, q->rear);
        for (int j = 0; j < q->size; j++) {
            int index = (q->front + j) % pq->max_size;
            p_queue_info_log(TAG, "  - Element %d: priority = %d, index = %d, data_index = %d", j, q->queue[index].priority, q->queue[index].index, q->queue[index].data_index);
        }
    }
    p_queue_mutex_unlock(&pq->queue_mutex);
}

/**
 * @brief Prints the contents of the priority queues in the priority queue system.
 * 
 * This function iterates through each priority queue in the given priority queue 
 * system and prints the elements in each non-empty queue. Each element's value is 
 * retrieved from the memory pool and printed based on the data index.
 * 
 * @param pq Pointer to the priority queue system (`p_queue_t`).
 * 
 * Each priority queue is printed in the following format:
 * "Priority <priority_number> queue: <element1> <element2> ... <elementN>"
 * 
 * The function assumes that the data stored in the queues are of type `int`, and 
 * accesses the memory pool accordingly.
 */
void p_queue_print(p_queue_t *pq) {
    if (!pq) {
        p_queue_debug_log(TAG, "Invalid priority queue pointer.");
        return;
    }
    // Acquire the mutex lock to ensure thread safety
    p_queue_mutex_lock(&pq->queue_mutex);
    for (int i = 0; i < pq->num_priorities; i++) {
        p_queue_node_t *queue = &pq->priority_queues[i];
        if (queue->size) {
            printf("Priority %d queue: ", i);
            for (int j = 0; j < queue->size; j++) {
                int index = (queue->front + j) % pq->max_size;
                int data_index = queue->queue[index].data_index;
                printf("%d ", *(int *)((char*)pq->memory_pool + data_index * pq->data_type_size));
            }
            printf("\n");
        }
    }
    p_queue_mutex_unlock(&pq->queue_mutex);
}
